
Filtre de Kalman 




1. Introduction : 

Le filtrage est la mise en forme d'un signal a des fins tres diversifiees. Pendant longtemps son role a 
consiste surtout dans 1' elimination des bruits superposes aux signaux utiles et sa mise en oeuvre a 
ete du ressort des electroniciens. 

Apres I'avenement de I'automatique et des calculateurs numerique, le filtrage est devenu un outil 
fondamental. II garantit Tequivalence analogique -numerique. Une fois la conversion numerique 
obtenue, le filtrage permet I'extraction d' informations utiles pour la reconnaissance des formes dans 
I'elaboration de la loi de commande des systemes industriels. 

II existe deux especes de filtrage appelees le filtrage frequentiel et le filtrage optimal. Le filtrage 
frequentiel comporte lui-meme deux fonctions : le filtrage analogique et le filtrage numerique. Le 
filtrage analogique demeure un element indispensable dans la synthese des filtres numeriques et un 
moyen de reduction de la bande passante pour le codage des signaux continus. De meme, le filtrage 
numerique resoudre le probleme de la mise en forme d'un signal et permet de realiser 
r interpolation et 1' extrapolation, et pent etre un filtre passe bande. 

Le filtrage optimal admet deux theories importantes appelees theorie de Wiener et theorie de 
Kalman. 

En 1950, Wiener a developpe sa theorie en traitant seulement les signaux stationnaires sous leur 
forme frequentielle. II resoudre le probleme de 1' elimination du bruit parasite inherent a la 
transmission et fait de I'estimation. Son inconvenient reside dans le non recursivite, ce qui rend un 
peu difficile la rapidite du systeme. 

En 1960, une theorie s'est developpee, dite theorie de Kalman, qui est une extension de la theorie de 
Wiener et represente plusieurs avantages. 

Filtre de Kalman : Le filtre de Kalman represente une extension de la theorie de Wiener 
applicable aux signaux non stationnaires sous leur forme temporelle en presence des conditions 
initiales et d' entrees deterministes. II resout le probleme de filtrage lineaire et se presente sous la 
forme d'un ensemble d'equations recurrentes plus faciles a resoudre sur calculateur numerique, ce 
qui explique le succes remarquable de I'approche proposee par Kalman. Sa realisation fournit non 
seulement I'estimee optimale, mais aussi la variance de I'erreur de I'estimation. C'est un 
observateur donnant la prediction ou I'estimation du vecteur d'etat. Lorsque les variances des 
bruits sont connues, c'est le meilleur observateur lineaire, de plus, si les bruits sont blancs 
gaussiens, lineaires ou non, c'est lui dont la variance de I'erreur d'estimation est la plus faible. 

Le filtre de Kalman s' applique au systeme dynamiques lineaires, connus ou discrets, ont le bruit de 
mesure est blanc. 

L'estimation de Kalman et ses nombreuses derivees ont ete developpee avec succes dans le monde 
de I'aerospatial a de nombreux problemes, comme la poursuite radar, I'estimation de trajectoires 
d'orbites... [1] 

Apres la parution des publications fondamentales de Kalman et Bucy pour la theorie de filtrage 
lineaire, un nombre tres important de travaux fut consacre a ce probleme, chaque auteur decrivant 
son algorithme en exploitant des methodes comme les moindres carrees, le maximum de 
vraisemblance et d'autres methodes statistiques classiques. Cependant, ces methodes statistiques 



3 




Filtre de Kalman 




laissent dans rombre la structure probabiliste du probleme de filtrage, structure qui est 
fondamentale. [2] 

2. Derivation du filtre de Kalman : 

2.1 Formulation du probleme : 

Le filtre de Kalman est caracterise par les mesures qui sont generes par un systeme dynamique qui 
est presente par un certain modele [7]. Mathematiquement le filtre de Kalman pent etre formule par 
deux equations : 

L'equation d'etat, qui presente revolution d'etat d'un objet durant le temps et I'equation de mesure, 
qui relie les mesures a cet etat. 

Nous considerons un systeme dynamique lineaire et discret, l'equation d'etat est donnee par : 

Xk+i = FkXk + GkUk + Wk (1.1) 

> Xk est le vecteur d'etat qui incluse les quantites a estimer, comme la position, la vitesse et 
I'acceleration. 

> Fk est la matrice de transition. Elle decrit revolution dans le temps du vecteur d'etat, en 
absence de I'entree de controle et du bruit. 

> Uk est I'entree de controle ou le processus generateur, elle est supposee etre connue. 

> Gk est une matrice qui decrit comment I'entree a I'instant k, contribue au vecteur d'etat a 
I'instantk+l. 

> Wk est le bruit de processus suppose independant, blanc et de moyenne nuUe. 
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Bruit blanc additif v(k) 



Figure 1.1 : Modele d'observation 



Zk = HkXk + Vk 

Ou 

> Hk est la matrice de mesure. 

> Vk est le bruit de mesure suppose independant de Wk, blanc et de moyenne nuUe. 



(1.2) 
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E[wk ] = (1.3) 

E[vk] = (1.4) 

E[vkw/"] = 0; V k, 1 (1.5) 

eKw/"] = Qk6(k, 1) (1.6) 

E[v^vJ] = Rk6(k, 1) (1.7) 

et 

E[xowJ] = V k (1.8) 

E[xovJ] = Vk (1.9) 

Qk et Rk etant respectivement les matrices de covariance du bruit de processus et du bruit de 
mesure. 

Le probleme consiste a estimer le vecteur d'etat XkCompte tenu des informations disponibles a 
I'instant n, posterieur, anterieur, ou identique a I'instant k. 

On pent considerer les cas suivants: [2] 

• k=n: il s'agit dans ce cas de determiner une estimee de I'etat, compte tenu de toutes les 
mesures disponibles a I'instant considere n. c'est le cas du filtrage. 

• k<n: en utilisant les mesures disponibles jusqu'a I'instant n, on essaye d'estimer I'etat a un 
instant k anterieur a n. on fait alors un lissage ou une interpolation. 

• k>n: il s'agit dans ce cas d'estimer la valeur du vecteur d'etat a un instant k dans le futur. 
c'est ce qu'on appelle la prediction ou I'extrapolation. 

Nous noterons pour ces differents cas, Testimee par: X^+i/n, c'est a dire I'estimee a I'instant k+1 
compte tenu des informations disponibles a I'instant n. 

L'estimee optimal au critere de I'erreur quadratique moyenne minimale est donnee par I'esperance 
mathematique conditionnelle notee comme suit: [5] 

Xk+l/n = E[xk+i/zi Zn] (1.10) 

L'evolution du vecteur d'etat estimee Xk+l/n ? est caracterisee par la matrice de transition F^- En 
effet en substituant I'equation (1.1) dans (1.10), on obtient: 

E[xk+i/zi Zk] = FkE[xk/zi,...,Zk] + GkE(uk+i/zL. -.Zk] + 

E[vk+i/zi, ... , Zk] (1.11) 
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Soit encore : 

Xk+i/k = Fk Xk/k + GkUk (1.12) 

Etant donne que : 

[Vk+l 



z(l) 



Zk 



= (1.13) 



I'objectif est d'avoir une estimation recursive du vecteur d'etat qui, a partir d'une estimation a 
I'instant k, nous foumisse une nouvelle estimation a I'instant k+l,en utilisant la mesure a cet instant. 
On adoptera une estimation lineaire de la forme: 

^k+l/k+1 = Xk+l/k + Kk+i [zk+i — Hk+iXk+ 1/kJ (1.14) 

La relation (1.14) exprime le fait que la nouvelle estimee du vecteur d'etat a I'instant k+1, est une 
mise a jour de I'estimee a I'instant k. cette mise a jour tient compte de I'ecart entre la mesure 
effective et la mesure predite. 

Nous devons done determiner le poids Ki^_^;[ a accorder a cette mise a jour de sorte que I'erreur au 
sens des moindres carres, entre le vecteur d'etat et son estimee, soit minimale. Le parametre K^+i 
est appele gain du filtre de Kalman. 

L'erreur d'estimation est definie par: 

Xk+i = Xk+i — Xk+i/k+i (1-15) 

II s'agit done de minimiser la quantite: 

Pk+i/k+1 = E[(Xk+i - Xk+ i/k+i)(Xk+i - Xk+ 1/k+i) ] = E[xk+iXi^+iJ (1-16) 

fk+l/k+1 ^st appelee matrice de covariance de l'erreur d'estimation. Le filtre de Kalman est done 
un filtre a minimum de variance. C'est par abus de langage que Ton parle de minimiser cette 
quantite matricielle. Nous y reviendrons dans le prochain paragraphe. 

2.2 Expression de la matrice de covariance : Pk+i/k+i [5] 

Nous allons, a partir des equations d'etat et de la relation lineaire de I'estimation exprimer la matrice 
de covariance P^+i/k+i en fonction du gain Kj^+x- 

Reprenons les relations (1.1) et (1.12). 
Xk+i = FkXk + GkUk + Wk 
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^k+l/k — Fk ^k/k + ^kUk 
En les soustrayant, il vient: 

Xk+i - Xk+i/k = Fk[xk - Xk/k] + Wk (1.17) 

D'apres la relation (1.16) definissant Pk+i/k+i, on a: 

Pk+i/k = E[(Xk+i - Xk+ i/k)(Xk+i - Xk+ i/\,y] (1.18) 

D'ou, en prenant les esperances mathematiques des deux membres de I'equation (1.17) 

Pk+i/k = FkE[(Xk - Xk/k)(Xk - Xk/k)'']Fj + E[(wkwJ)] (1.19) 

Cette relation peut se simplifier en tenant compte de la relation (1.6) et du fait que: 

E[(Xk+i - Xk/k)(Xk+i - Xk/k)"^] = Pk/k (1-20) 

II vient alors: 

I\+l/k = F"kPk/kF"k + Qk (1.21) 

Nous devons a ce stade du calcul exprimer Pk+i/k+l ^^ fonction de Pk+i/k- 

Pour ce faire, reprenons I'expression (1.15) de I'erreur Xk+iet rempla^ons I'estimee Xk+i/k+i par 

son expression donnee par la relation (1.14), on obtient: 

Xk+l/k+l = Xk+i — Xk+i/k — Kk+i[Hk+iXk+i + Vk+i — Hk+iXk+ l/kj 

= - Kk+iHk+i)(xk+i - Xk+ i/k) - Kk+iVk+i (1.22) 

Ou Pk+i/k+i = E[xk+i/k+iXk+i/k+iJ (1.23) 

Si on substitue I'expression (1.22) dans la relation (1.23), il vient en tenant compte de la relation 
que: (AB)"^ = B^A"^ que : 

Pk+i/k+1 = ~ Kk+iHk+i)E[(Xk+i — Xk+i/k)(Xk+i -Xk+i/k) ](I — Kk+iHk+i) 

+ Kk+iE[Vk+ivJ+i]Kj+i (1.24) 

Soit encore: 

Pk+i/k+1 = — Kk+iHk+i)Pk+ i/k(I — Kk+iHk+i) + Kk+iRk+iKj^+i (1.25) 
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Pk+l/k+l — Pk+l/k ~ Kk+lHk+lPk+1/k ~ Pk+1/kHk+iKk+i + 

Kk+i[Hk+iPk+i/k+iHk+i + Pk+i]Kk+i (1.26) 

2.3 Expression du gain du filtre de Kalnnan: [5] 

Nous devrons chercher I'expression du gain Kk+i qui minimise le critere 

J = E[xJ+i/k+iXk+i/k+i] (1.27) 

Notons au passage que nous avons: 

Trace Pk+i/k+i = ^[^k-\- i/k+i^k+i/k+ij = J (1.28) 

La relation (1.26) est une forme quadratique en Kk+i- Nous obtiendrons la valeur du gain optimal 
en resolvant I'equation suivante: 

d](k) 

= 
aK(k) 

L'optimisation de ce probleme peut etre faite de differentes manieres. L'une de ces manieres se base 
sur les fomiules du calcul differentiel des matrices. En utilisant les relations suivantes: 

'^^^7/^"^^ = B^ (AB doit etre carre) (1.29) 

a(Trace(ACA'^)) 

— = 2AC (C doit etre symetrique) (1.30) 

oA 

On obtient 

a(TraceP,+ i/,+i) _ 

^^^— --2(Hk+iPk+i/k) + 

2Kk+i(Hk+iPk+i/kHk+i + Rk+i) (1.31) 

Soit encore : 

~0 - Kk+iHk+i)Pk+i/kHk+i + Kk+iRk+i = (1.32) 

D'ou 

Kk+i = Pk+i/kHk+i [Hk+iPk+i/kHk+i + Rk+i]~ (1.33) 

Et 
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Pk+l/k+l = Pk+l/k ~ Pk+ l/kHk+iHk+iPk+ 1/kHk+i + Rk+iHk+iPk+ 1/k (1-34) 

Dans le cas continu, cette equation prend la forme d'une equation differentielle de Ricatti. 

C'est pour cette raison, et par analogic que la relation (1.34) est appelee equation de Ricatti 
discrete. 

Elle pent etre reecrite sous la forme suivante: 

Pk+l/k+l = Pk+l/k ~ Kk+iHk+iPk+i/k = [I — Kk+iHk+i]Pk+i/k (1.35) 

La matrice Kk+iHk+iPk+i/k ^^t definie positive ou a la rigueur definie semi-positive. 
Nous pouvons en outre ecrire I'equation recursive de mise a jour de I'estimation comme suit: 

Xk+i = Xk + Kk[terme de correction] (1.36) 

II apparait ainsi que: 

1. Pour R^+i constant, si P^+i faible, le gain sera faible. Ainsi, on fera davantage confiance a 
I'estimation obtenue a partir du modele. Si par contre P^+i est eleve, ce qui traduit notre 
faible confiance dans le modele K sera eleve et la contribution du terme de correction 
pondere par le gain, sera plus forte. On se basera done davantage sur les mesures pour la 
mise a jour. 

2. Pour Pk+i constant, si R^+i est faible, nous aurons des mesures faiblement bruitees. La 
valeur elevee du gain donnera plus de poids aux mesures. Si R^+i est eleve,le gain sera 
faible et le poids du deuxieme terme sera plus faible. 

2.4 Initialisation du filtre de Kalman: [3] 

Pour pouvoir utiliser I'ensemble des equations recurrentes constituant le filtre de Kalman, on doit 
choisir les conditions initiales: Xq/q et Po/o-On pent prendre: 

xo/o = E[xo] (1.37) 

Po/0 = Po = E[xo - Xo/o)(xo - Xo/o) ] (1-38) 

Le choix des valeurs initiales est delicat. En effet, un mauvais choix de Xq, c'est a dire, a la limite 
une valeur arbitraire n'est pas catastrophique en ce sens que I'algorithme excite par les mesures 
apportera les corrections necessaires. Par contre le traitement des mesures n'ameliore pas la 
covariance de I'erreur au fur et a mesure de son traitement. 

2.5 Notion d'innovation: [5] 

Reprenons I'equation (1.14) de I'estimation recursive: 
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^k+l/k+l — ^k+l/k + Kk+l[2^k+l ~ Hk+iXk+i/k] 

Zk+i represente la mesure effective et H^+iXk+l/k 1^ prediction de la mesure. Si cette prediction 
est prediction est parfaite, la correction apportee par la quantite 

^k+1 = Zk+i - Hk+iXk+i/k (1.39) 

sera nuUe. On appelle z^+i I'innovation. On pent montrer que si le filtre est optimal, le processus 
Zk+i est un bruit blanc de valeur moyenne nulle, c'est a dire qu'il ne contient plus d'informations 
pouvant enrichir la mise a jour de I'etat. 

Ainsi, en testant la « blancheur » de I'innovation on pent apprecier le degre d'optimalite et done les 
performances du filtre. 

3. Mise en oeuvre du filtre de Kalman [2] 

On pent resumer I'ensemble des equations recurrentes constituant le filtre de Kalman dans les etapes 
suivantes : 

3.1 Prediction: 

1. A partir du vecteur d'etat a I'instant anterieur, la prediction du vecteur d'etat est 
effectuee selon: 

Xk+i/k = Fk Xk/k + GkUk (1.40) 

2. Calcul de la matrice de covariance correspondant a I'etat predit: 

Pk+i/k = FkPk/kFj + Qk (1-41) 

3.2 Estimation: 

1 . Mise a jour du vecteur d'etat: 

^k+i/k+1 = Xk+i/k + Kk+iZk+i (1-42) 

Ou Zk+i est I'innovation definie en (1.39) et Kk+iest le gain de Kalman donnee par: 

Kk+i = Pk+i/kHk+iSk+i (1-43) 

S(k + 1) est appelee matrice de covariance de I'innovation. EUe est donnee par: 

Sk+1 = Hk+iPk+i/kHk+i + Rk+i (1.44) 
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2. Mise a jour de la matrice de covariance de I'etat estime 
la matrice de covariance correspondant a (1.42) est calculee selon: 



Pk+l/k+1 — Pk+l/k ~ Kk+iSk+iK^+i 



(1.45) 



On peut resumer ralgorithme de filtre de Kalman dans le cycle suivant en cinq etapes de base qui 
sont: 

> Extrapoler la position et la vitesse de la cible. 

> Extrapoler les erreurs d'estimation 

> Calculer le gain du filtre. 

> Mettre a jour I'estimation de vecteur d'etat. 

> Mettre a jour I'estimation pour les nouvelles donnees (mesure). 

Le schema de la figure 1.2 resume un cycle de calcul de I'etat estime d'un systeme lineaire par le 
filtre de Kalman. 



Zk+i 



Zk+i 



s 



r^ 



Entiees 
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Uk 



^k+l/k+l 
7^ 



Soitie 



^k/k 



Figure 1.2 : Un cycle pour I'estimation de I'etat d'un systeme lineaire (filtre de Kalman) 



4. Les modeles cinematiques : 

Les modeles d'etat cinematiques sont definit par la mise de quelques derivations de position a Zero. 
II existe deux modeles: modele de deuxieme ordre (white noise acceleration) et le modele de 
troisieme ordre (Wiener process acceleration). [6] 

4.1 Modele a vitesse constante (deuxieme ordre) 

Ce modele est plus adapte au mouvement d'une cible qui se deplace a une vitesse constante. Un 
objet deplace avec une vitesse constante, son mouvement est presente par I'equation: 
Xk = 
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La position evolue, en absence du bruit, selon un polynome (dans ce cas de ordre), ce modele est 
appele le modele polynomiale. En pratique, la vitesse subit des changements aleatoires, qui peuvent 
etre modelises par un bruit blanc d moyenne nuUe. 

Si on suppose que la poursuite se fait dans un plan (X,Y), le vecteur d'etat relatif a la direction x 
sera forme de deux composantes qui sont la position et la vitesse dans cette direction: 

Xk = [% x]T" (1.46) 

L'equation d'etat pour un filtre de Kalman du deuxieme ordre est: 

Xk+i = FrXr + GkUk (1.47) 

La matrice de transition est donnee par: 
[1 Tl 



Lo iJ 



(1.48) 



Oil T presente le temps entre deux mesures, appelee aussi le temps de mise a jour. 

Le terme G^Uk est introduit afin de prendre en consideration les fluctuations de vitesse. 

Uk est suppose etre une valeur scalaire, blanche de moyenne nulle et de variance O p , qui 

represente I'acceleration supposee constante durant la k^™^ periode d'echantillonnage. On a: 



G = 



j2 



(1.49) 



E[u(k)uT(l)] = a%6(k, 1) (1.50) 



etQ(k) = K[Gu(k)uT(l)GT] = 



l'T'4 lyS 
4 2 



(1.51) 



II n'existe pas de regie pour le choix de Qp . On pent cependant le choisir de fagon empirique de 
I'ordre de grandeur de I'acceleration maximal a^ :0.5 a^ < Qp < a^ 

4.2 Modele a acceleration constante (troisienne ordre) 

Ce modele est approprie au mouvement d'une cible manoeuvrante. II suppose que I'acceleration est 
constante et utilise un bruit de processus pour modeliser ses fluctuations eventuelles. L'equation 
d'etat correspondant a ce modele est similaire a l'equation (1.47) . 

Les composantes du vecteur d'etat x(k) sont dans ce cas: la vitesse et I'acceleration. 

Xk = [x,x,x]'^ (1.52) 
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La matrice de transition Fj^ est donnee par: 
1 



F = 



1 T -T^ 
2 

1 T 
1 



(1.53) 



Les fluctuations de I'acceleration sont modelisees par le bruit de processusG(k)v(k), ou v(k) 
represente I'increment de I'acceleration durant la k^"^^ periode d'observation. 



11 est suppose une sequence blanche de moyenne nuUe avec une varianc O yY avec: 



G = 



T2 

— T 1 

2 



(1.54) 



La matrice de covariance du bruit de processus est donnee par 



Q(k) = 



1 j4 
4 

2 

— T 

■ 2 



r2n 

2 2 



1^3 T^ 



T 
1 



(1.55) 



Les changements de I'acceleration sont de I'ordre de a y. Pour avoir un modele proche du modele a 
acceleration constante il faut choisir une intensite de bruit Gy petite de fagon que les changement de 
Tacceleration A a^ pendant une periode restent petits par rapport a I'acceleration actuelle. 

En pratique en prendra: 0.5 A a^ < o^ < A a^ . 



til 



